#include "sensor_imu.h"
#include <iostream>
#include <vector>
#include <fstream>
#include <stdio.h>
#include <queue>
#include <stdlib.h>
#include <string.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64MultiArray.h"


using namespace std;
RTIMUSettings *settings = new RTIMUSettings("RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
float temp_gyros;
RTIMU_DATA gyroscope;



int main(int argc, char **argv)
{

  ros::init(argc, argv, "sensor_imu_node");
  ros::NodeHandle imu_handle;

  Imu_Init();//gyroscope  init
  ros::Rate loop_rate(10000);

  ros::Publisher sensor_imu_data  = imu_handle.advertise<std_msgs::Float64MultiArray>
          ("sensor/imu", 5);


  while (imu_handle.ok())
  {
      std_msgs::Float64MultiArray imu_data;
      gyroscope= ReadImuData( );
      temp_gyros=gyroscope.fusionPose.z()*180/3.14;
      imu_data.data.push_back(temp_gyros);
      sensor_imu_data.publish(imu_data);
      //printf(" temp_gyros:%f\n",temp_gyros);
      loop_rate.sleep();
      ros::spinOnce();
  }

}


void Imu_Init()
{

			int sampleCount = 0;   
			int sampleRate = 0;   
			uint64_t rateTimer;  
			uint64_t displayTimer; 
			uint64_t now;
	
		
			if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) {
				printf("No IMU found\n");
				exit(1);
			}
			
			//	This is an opportunity to manually override any settings before the call IMUInit	
			//	set up IMU	
			 imu->IMUInit();
	
			//	this is a convenient place to change fusion parameters
			imu->setSlerpPower(0.02);	
			imu->setGyroEnable(true);  
			imu->setAccelEnable(true);	 
			imu->setCompassEnable(true);

	     
	
			
}

RTIMU_DATA  ReadImuData( )
{
   while(1)
   {
    usleep(imu->IMUGetPollInterval() * 1000);

		while(imu->IMURead()){

		RTIMU_DATA imuData = imu->getIMUData(); 
		return imuData;
	
		}
   	}
 
}






